#include "zuc_nml_rob.hh"
#include "libnml/cms/cms.hh"

void RobJointVal_update(CMS* cms, RobJointVal* x)
{
    for (size_t i = 0; i < RobJointVal::MAX_AXIS; i++) { cms->update(x->val[i]); }
}
void ZucPose_update(CMS* cms, ZucPose* x)
{
    cms->update(x->tran.x);
    cms->update(x->tran.y);
    cms->update(x->tran.z);
    cms->update(x->a);
    cms->update(x->b);
    cms->update(x->c);
    cms->update(x->u);
    cms->update(x->v);
    cms->update(x->w);
}

void DynParam_update(CMS* cms, DynParam* x)
{
    cms->update(x->kineticK, 14 * ZUCMOT_MAX_JOINTS);
    cms->update(x->dynaticK, 72);
}

void DHParam_update(CMS* cms, DHParam* x)
{
    cms->update(x->alpha, ZUCMOT_MAX_JOINTS);
    cms->update(x->a, ZUCMOT_MAX_JOINTS);
    cms->update(x->d, ZUCMOT_MAX_JOINTS);
    cms->update(x->joint_homeoff, ZUCMOT_MAX_JOINTS);
}

void RobotDH_update(CMS* cms, RobotDH* x)
{
    RobJointVal_update(cms, &x->alpha);
    RobJointVal_update(cms, &x->a);
    RobJointVal_update(cms, &x->d);
    RobJointVal_update(cms, &x->joint_homeoff);
}

void TIOInfoUpdate(CMS* cms, TIOInfo* x)
{
    cms->update(x->diNum);
    cms->update(x->doNum);
    cms->update(x->aiNum);
    cms->update(x->aoNum);
}

void RobotModelCapab_update(CMS* cms, RobotModelData* x)
{
    cms->update(x->model);
    cms->update(x->payload);
    DHParam_update(cms, &x->nominalDH);
    DynParam_update(cms, &x->dynParam);
    cms->update(x->maxTrajVel);
    cms->update(x->maxTrajAcc);
    cms->update(x->maxJointPosLimit, ZUCMOT_MAX_JOINTS);
    cms->update(x->minJointPosLimit, ZUCMOT_MAX_JOINTS);
    cms->update(x->maxJointVel, ZUCMOT_MAX_JOINTS);
    cms->update(x->maxJointAcc, ZUCMOT_MAX_JOINTS);
    cms->update(x->joint_err_limit, ZUCMOT_MAX_JOINTS);
    cms->update(x->motorRatedCurr, ZUCMOT_MAX_JOINTS);
    cms->update(x->motorRatedTorq, ZUCMOT_MAX_JOINTS);
    cms->update(x->motorRatedTransRatio, ZUCMOT_MAX_JOINTS);
    cms->update(x->motorSupplierId, ZUCMOT_MAX_JOINTS);
    cms->update(x->defShrinkenPose, ZUCMOT_MAX_JOINTS);
    cms->update(x->defStretchedPose, ZUCMOT_MAX_JOINTS);
    cms->update(x->defInitalJPos, ZUCMOT_MAX_JOINTS);
    cms->update(x->gearboxMaxTorque, ZUCMOT_MAX_JOINTS);
    TIOInfoUpdate(cms, &x->tioInfo);
    cms->update(x->minMomentum);
    cms->update(x->maxMomentum);
    cms->update(x->minRobotPower);
    cms->update(x->maxRobotPower);
    cms->update(x->robotPowerLimit, 2);
    cms->update(x->robotMomentumLimit, 2);
    cms->update(x->robotEndforceLimit, 2);
    cms->update(x->robotVelocityLimit);
}

void Payload_update(CMS* cms, Payload* x)
{
    cms->update(x->m);
    cms->update(x->p, 3);
}

void VerpSimulationSafeinformation_Update(CMS* cms, zuc_traj_verp_simulation_safe_information* verp_safe_information)
{
    for (int i = 0; i < 6; i++)
    {
        cms->update(verp_safe_information->plane[i].enable);
        cms->update(verp_safe_information->plane[i].planePoints.x);
        cms->update(verp_safe_information->plane[i].planePoints.y);
        cms->update(verp_safe_information->plane[i].planePoints.z);
        cms->update(verp_safe_information->plane[i].plane_Xyz.x);
        cms->update(verp_safe_information->plane[i].plane_Xyz.y);
        cms->update(verp_safe_information->plane[i].plane_Xyz.z);
    }
    cms->update(verp_safe_information->tool_cone_dir.x_y_cone);
    cms->update(verp_safe_information->tool_cone_dir.cone_xyz.x);
    cms->update(verp_safe_information->tool_cone_dir.cone_xyz.y);
    cms->update(verp_safe_information->tool_cone_dir.cone_xyz.z);
    cms->update(verp_safe_information->tool_cone_dir.tool_dir_xyz.x);
    cms->update(verp_safe_information->tool_cone_dir.tool_dir_xyz.y);
    cms->update(verp_safe_information->tool_cone_dir.tool_dir_xyz.z);
    cms->update(verp_safe_information->tool_cone_dir.z_cone);
}

void TIOStatusUpdate(CMS* cms, TIOStatus* x)
{
    cms->update(x->tio_version);
    cms->update(x->has_tio);
    cms->update(x->UIKey_free);
    cms->update(x->UIKey_point);
    cms->update(x->UIKey_pause_resume);
    cms->update(x->DI_buffer);
    cms->update(x->DO_buffer);
    cms->update(x->Lamp_color);
    cms->update(x->AI_range, 4);
    cms->update(x->AI, 4);
    cms->update(x->AO, 4);
    cms->update(x->torque, CART_DIM);
}

void ModRtuComm_update(CMS* cms, ModRtuComm* x)
{
    cms->update(x->slaveId);
    cms->update(x->baudrate);
    cms->update(x->databits);
    cms->update(x->stopbits);
    cms->update(x->parity);
}
void TIORSChnConfig_update(CMS* cms, TIORSChnConfig* x)
{
    cms->update(x->mode);
    ModRtuComm_update(cms, &x->rtuComm);
}

void TIOPinConfig_update(CMS* cms, TIOPinConfig* x)
{
    cms->update(x->diCfg);
    cms->update(x->doCfg);
    cms->update(x->aiCfg);
}

void TIOConfig_update(CMS* cms, TIOConfig* x)
{
    TIOPinConfig_update(cms, &x->pinCfg);
    TIORSChnConfig_update(cms, &x->rsChnCfg[0]);
    TIORSChnConfig_update(cms, &x->rsChnCfg[1]);
    cms->update(x->voutEna);
    cms->update(x->voutVol);
}

void Dynamics_update(CMS* cms, Dynamics* x)
{
    cms->update(x->ele_dynamics, 88);
    cms->update(x->friction_dynamics, 88);
    cms->update(x->max_err, 7);
    cms->update(x->mean_err, 7);
    cms->update(x->std_err, 7);
}

void GripperStateUnitUpdate(CMS* cms, GripperStateUnit* x)
{
    cms->update(x->sigName, 128);
    cms->update(x->slaveId);
    cms->update(x->type);
    cms->update(x->addr);
    cms->update(x->value);
    cms->update(x->freq);
    cms->update(x->count);
}

void FtConfig_update(CMS* cms, FtConfig* x)
{
    cms->update(x->enableFt);
    cms->update(x->enableNTrack);
    cms->update(x->ftUser);
    cms->update(x->ftReboundFK);
    cms->update(x->ftConstant);
}

void CaptureStatus_update(CMS* cms, CaptureStatus* x)
{
    cms->update(x->captStatus);
    cms->update(x->stateType);
    cms->update((unsigned char*)(&x->captRes), sizeof(StateUnion));
}

void SafeAtt_update(CMS* cms, SafeAtt* safeatt)
{
    cms->update(safeatt->conicalCenRpy.r);
    cms->update(safeatt->conicalCenRpy.p);
    cms->update(safeatt->conicalCenRpy.y);

    cms->update(safeatt->deviation);
}

ZUC_TRAJ_STAT::ZUC_TRAJ_STAT()
{
    linearUnits = 1.0;
    angularUnits = 1.0;
    cycleTime = 0.0;
    joints = 1;
    deprecated_axes = 1;
    axis_mask = 1;
    mode = ZUC_TRAJ_MODE_FREE;
    enabled = OFF;
    inpos = ON;
    queue = 0;
    activeQueue = 0;
    queueFull = OFF;
    id = 0;
    paused = OFF;
    scale = 0.0;
    rapid_scale = 0.0;

    ZERO_ZUC_POSE(position);
    ZERO_ZUC_POSE(actualPosition);

    velocity = 1.0;
    acceleration = 1.0;
    maxVelocity = 1.0;
    maxAcceleration = 1.0;

    ZERO_ZUC_POSE(dtg);
    distance_to_go = 0.0;
    kinematics_type = 0;
    motion_type = 0;
    current_vel = 0.0;
    feed_override_enabled = OFF;
    admittance_enabled = OFF;
}

void ServoMoveFilterParam_update(CMS* cms, ServoMoveFilterParam* x)
{
    cms->update(x->filterType);
    cms->update(x->mmf_buf_size);
    cms->update(x->mmf_ka);
    cms->update(x->mmf_kv);
    cms->update(x->mmf_kp);
    cms->update(x->nlf_vp);
    cms->update(x->nlf_ap);
    cms->update(x->nlf_jp);
    cms->update(x->nlf_vr);
    cms->update(x->nlf_ar);
    cms->update(x->nlf_jr);
    cms->update(x->lpf_cutoff_freq);
}
/*
*	NML/CMS Update function for ZUC_TRAJ_STAT
*	Automatically generated by NML CodeGen Java Applet.
*	on Sat Oct 11 13:45:17 UTC 2003
*/
void ZUC_TRAJ_STAT::update(CMS* cms)
{
    cms->update(linearUnits);
    cms->update(angularUnits);
    cms->update(cycleTime);
    cms->update(joints);
    cms->update(deprecated_axes);
    cms->update(axis_mask);
    cms->update((int*)&mode, 1);
    cms->update(enabled);
    cms->update(is_being_enabled);
    cms->update(inpos);
    cms->update(queue);
    cms->update(activeQueue);
    cms->update(queueFull);
    cms->update(id);
    cms->update(paused);
    cms->update(scale);
    cms->update(rapid_scale);

    ZucPose_update(cms, &position_desired);
    ZucPose_update(cms, &position);
    ZucPose_update(cms, &actualPosition);
    ZucPose_update(cms, &curSegTargetPose);
    cms->update(velocity);
    cms->update(acceleration);
    cms->update(maxVelocity);
    cms->update(maxAcceleration);
    ServoMoveFilterParam_update(cms, &servoMtnFltParam);

    cms->update(kinematics_type);
    cms->update(motion_type);
    cms->update(distance_to_go);
    ZucPose_update(cms, &dtg);
    cms->update(current_vel);
    cms->update(curr_tcp_trans_vel);
    cms->update(curr_tcp_angular_vel);
    cms->update(curr_elbow_vel);
    cms->update(vel_tcp_angular);
    cms->update(acc_tcp_linear);
    cms->update(acc_tcp_angular);
    cms->update(feed_override_enabled);
    cms->update(admittance_enabled);
    for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++) { cms->update(goalJointPos[i]); }
}
void ZUC_MOTION_STAT::update(CMS* cms)
{
    for (size_t i_joint = 0; i_joint < RobJointData<ZUC_JOINT_STAT>::MAX_AXIS; i_joint++) joint[i_joint].update(cms);
    traj.update(cms);
    sensor.update(cms);

    cms->update(robot_serial_num);
    RobotModelCapab_update(cms, &robot_model_data);

    cms->update(dragTcpSpeedLimit);
    cms->update(on_soft_limit);
    cms->update(next_to_limit);
    cms->update(protective_stop);
    cms->update(safetyguard_lock);
    cms->update(emergency_stop);
    cms->update(category_one_stop_program_pause);
    cms->update(estop_recovery);
    cms->update(powered_on);
    cms->update(is_powering_on);
    cms->update(percentage_mode_level);
    cms->update(is_reduce_mode);
    cms->update(full_dh_com);

    cms->update(elbow_center_offset);
    cms->update(elbow_sphere_radius);
    cms->update(dragNearLimit, ZUCMOT_MAX_JOINTS);
    cms->update(admit_axis_opt, CART_DIM);
    cms->update(admitCtrlConfig, CART_DIM);

    cms->update(payload, 4);
    cms->update(cur_payload_id);
    cms->update(base_frame_id);
    cms->update(currentToolId);
    cms->update(currentUserId);

    cms->update(identifyWithLoad);
    cms->update(identifyTrajIndex);
    cms->update(noloadIdentifyStat, 3);
    cms->update(onloadIdentifyStat, 3);
    cms->update(robotJointOffset, ZUCMOT_MAX_JOINTS);

    cms->update(percentage_mode_level1_val);
    cms->update(percentage_mode_level2_val);
    cms->update(identifyfrictionStat);
    cms->update(identifyfrictionstartposStat, 42);
    cms->update(identifyfrictionendposStat, 42);
    cms->update(backdriveStat);
    cms->update(forcelimitvalueStat);
    cms->update(jointcollisionsensitivityStat, ZUCMOT_MAX_JOINTS);
    cms->update(jointfrictioncompensateStat, ZUCMOT_MAX_JOINTS);
    cms->update(collisiondetectionenableStat);
    cms->update(dynamicsIdentifyStat);
    cms->update(dynamicsIdentifyTrajPara, 250);
    cms->update(dynamicsIdentifyStartPos, ZUCMOT_MAX_JOINTS);

    cms->update(steppingWait);
    RobotDH_update(cms, &dhParam);

    cms->update(torqueSensorSoftLimit, CART_DIM);
    cms->update(drag_dead_zone, CART_DIM);
    cms->update(torqueSensorFilter);
    for (int i = 0; i < 6; i++) { FtConfig_update(cms, &admitFtConfig[i]); }
    cms->update(drag_dead_zone, 6);

    cms->update(velCompliantCtrl, 5);
    cms->update(compliantCondition, CART_DIM);
    cms->update(compliantType);
    cms->update(compliantEnable);
    cms->update(ftFrame);
    cms->update(disableForceCtrl);
    cms->update(admitCtrlEnaFlag);

    for (int i = 0; i < 8; i++) CaptureStatus_update(cms, &captureStatus[i]);

    cms->update(utilJointPos, ZUCMOT_MAX_JOINTS);
    ZucPose_update(cms, &utilCartPos);
    Payload_update(cms, &utilPayload);
    Dynamics_update(cms, &utilDynamics);
    SafeAtt_update(cms, &safeatt);
    cms->update(tcpError, 3);
    cms->update(is_momentumLimited);
    cms->update(is_robot_power_limited);
    cms->update(is_safezone_limited);
    cms->update(is_tcpvel_limited);

    cms->update(is_jointvel_limited, ZUCMOT_MAX_JOINTS);
    cms->update(three_position_enable_limit);

    cms->update(errAddLine);
    cms->update(safetyCRC);
    cms->update(maintenance_status);

    ZucPose_update(cms, &carte_elbow_pos_cmd);
    cms->update(robot_real_power);
    cms->update(robot_real_momentum);
    cms->update(torque_control_enable);

    cms->update(shrunkenPose, ZUCMOT_MAX_JOINTS);
    cms->update(stretchedPose, ZUCMOT_MAX_JOINTS);
    cms->update(initialPose, ZUCMOT_MAX_JOINTS);  //
    cms->update(initPoseTol);
    cms->update(homeVel);
}

ZUC_JOINT_STAT::ZUC_JOINT_STAT()
{
    jointType = ZUC_LINEAR;
    units = 1.0;
    backlash = 0.0;
    minPositionLimit = -1.0;
    maxPositionLimit = 1.0;
    minFerror = 1.0;
    maxFerror = 1.0;
    ferrorCurrent = 0.0;
    ferrorHighMark = 0.0;
    output = 0.0;
    input = 0.0;
    velocity = 0.0;
    inpos = 1;
    homing = 0;
    homed = 0;
    enabled = 0;
    fault = 0;
    minSoftLimit = 0;
    maxSoftLimit = 0;
    minHardLimit = 0;
    maxHardLimit = 0;
}
void ZUC_JOINT_STAT::update(CMS* cms)
{
    cms->update(servo_version);
    cms->update(clsn_sensitivity);
    cms->update(first_poweron_date);
    cms->update(jointType);
    cms->update(units);
    cms->update(backlash);
    cms->update(minPositionLimit);
    cms->update(maxPositionLimit);
    cms->update(maxFerror);
    cms->update(minFerror);
    cms->update(maxVel);
    cms->update(maxAcc);
    cms->update(err_limit);
    /*! \todo FIXME - homingVel has been superceded */
    cms->update(ferrorCurrent);
    cms->update(ferrorHighMark);
    cms->update(desired_pos);
    cms->update(output);
    cms->update(input);
    cms->update(velocity);
    cms->update(inpos);
    cms->update(homing);
    cms->update(homed);
    cms->update(fault);
    cms->update(enabled);
    cms->update(minSoftLimit);
    cms->update(maxSoftLimit);
    cms->update(minHardLimit);
    cms->update(maxHardLimit);
    cms->update(servo_warn_8384_cnt);
    cms->update(servo_warn_8384_daliy_cnt);
    cms->update(servo_warn_8385_cnt);
    cms->update(servo_warn_8385_daliy_cnt);
    cms->update(servo_warn_8387_cnt);
    cms->update(servo_warn_8387_daliy_cnt);
    cms->update(servo_warn_8388_cnt);
    cms->update(servo_warn_8388_daliy_cnt);
    cms->update(servo_warn_7001_cnt);
    cms->update(servo_warn_7001_daliy_cnt);

    cms->update(cmd_tor);
    cms->update(act_tor);
}
